// --------------------------------------------------------------------------------------------------------------------
// <copyright file="JointState.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Hardware.DAC
{
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Algorithms;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// The joint state class
    /// </summary>
    [DataContract]
    public class JointState : IDeviceState
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="JointState"/> class.
        /// </summary>
        public JointState()
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="JointState"/> class.
        /// </summary>
        /// <param name="that">The copy constructor initializer</param>
        public JointState(JointState that)
        {
            this.CurrentAccelerationInRadiansPerSecondSquared = that.CurrentAccelerationInRadiansPerSecondSquared;
            this.CurrentVelocityInRadiansPerSecond = that.CurrentVelocityInRadiansPerSecond;
            this.DeviceId = that.DeviceId;
            this.DeviceType = that.DeviceType;
            this.MaxPositionInRadians = that.MaxPositionInRadians;
            this.MinPositionInRadians = that.MinPositionInRadians;
            this.Model = that.Model;
            this.Pose = that.Pose;
            this.PositionInRadians = that.PositionInRadians;
            this.PositionPrecisionInRadians = that.PositionPrecisionInRadians;
            this.TimestampInMilliseconds = that.TimestampInMilliseconds;
            this.TimestampInTicks = that.TimestampInTicks;
        }

        /// <summary>
        /// Gets or sets the position of the joint in radians based from start position
        /// </summary>
        [DataMember]
        public double PositionInRadians { get; set; }

        /// <summary>
        /// Gets or sets max position in radians
        /// </summary>
        [DataMember]
        public double MaxPositionInRadians { get; set; }

        /// <summary>
        /// Gets or sets max position in radians
        /// </summary>
        [DataMember]
        public double MinPositionInRadians { get; set; }

        /// <summary>
        /// Gets or sets current velocity of joint in rads/sec
        /// </summary>
        [DataMember]
        public double CurrentVelocityInRadiansPerSecond { get; set; }

        /// <summary>
        /// Gets or sets current acceleration of join in rads/sec^2
        /// </summary>
        [DataMember]
        public double CurrentAccelerationInRadiansPerSecondSquared { get; set; }

        /// <summary>
        /// Gets or sets the precision of the position reading in radians
        /// </summary>
        [DataMember]
        public double PositionPrecisionInRadians { get; set; }

        /// <summary>
        /// Gets or sets the timestamp for the state.
        /// </summary>
        [DataMember]
        public long TimestampInTicks { get; set; }

        /// <summary>
        /// Gets or sets the timestamp in milliseconds (as opposed to hardware-dependent ticks).
        /// </summary>
        [DataMember]
        public long TimestampInMilliseconds { get; set; }

        /// <summary>
        /// Gets or sets the device identifier
        /// </summary>
        [DataMember]
        public int DeviceId { get; set; }

        /// <summary>
        /// Gets or sets the device model information
        /// </summary>
        [DataMember]
        public string Model { get; set; }

        /// <summary>
        /// Gets or sets the type of the device the sensor is related
        /// </summary>
        [DataMember]
        public DeviceType DeviceType { get; set; }

        /// <summary>
        /// Gets or sets the pose for the device/sensor
        /// </summary>
        [DataMember]
        public Pose Pose { get; set; }

        /// <summary>
        /// Called to interpolate between the start and end states by the time provided by the interpolation timer's desiredTime.        
        /// </summary>        
        /// <param name="startTime">The start time for interpolation</param>
        /// <param name="endTime">The end time for interpolation</param>
        /// <param name="desiredTime">The desired time for interpolation</param>
        /// <returns>The interpolated joint state</returns>
        /// <param name="startState">The start state for interpolation</param>
        /// <param name="endState">The end state for interpolation</param>
        public static JointState Interpolate(long startTime, long endTime, long desiredTime, JointState startState, JointState endState)
        {            
            JointState interpolatedState = new JointState(startState);            
            interpolatedState.PositionInRadians = Pose2D.LinearInterpolateHeading(startTime, endTime, desiredTime, startState.PositionInRadians, endState.PositionInRadians);
            interpolatedState.TimestampInMilliseconds = (long)Interpolator.LinearInterpolate(startTime, endTime, desiredTime, startState.TimestampInMilliseconds, endState.TimestampInMilliseconds);
            interpolatedState.TimestampInTicks = (long)Interpolator.LinearInterpolate(startTime, endTime, desiredTime, startState.TimestampInTicks, endState.TimestampInTicks);

            return interpolatedState;
        }
    }
}
